﻿using System;
using System.Timers;
using MECopter.Joystick;
using Sharp3D.Math.Core;


namespace MECopter.Quadrocopter
{
    class ManualController : Controller
    {     
        #region Fields
        private Timer ctrlTimer;
        private JoystickCapabilities jc;
        private JoystickState js;
        #endregion


        #region Constructor
        public ManualController()
        {
            this.Values = new ControllValues();

            jc = Joystick.Joystick.GetJoystickCapabilities(JoystickIndex.JOYSTICK1);

            ctrlTimer = new Timer();
            ctrlTimer.Elapsed += ctrlTimer_Elapsed;

            ctrlTimer.Interval = MECopter.Configuration.GroundControl.ConfigurationManager.CurrentControllerConfig.intervalTime;
        }
        #endregion

        #region Eventhandler
        private void ctrlTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            js = Joystick.Joystick.GetState(JoystickIndex.JOYSTICK1);
     
            Values.Speed = (float)(((((float)js.PositionAxisR * 2) / jc.MaxAxisR) - 1) * MathFunctions.PI) + (float)MathFunctions.PI; //Speed
            Values.Yaw = (float)(((((float)js.PositionAxisZ * 2) / jc.MaxAxisU) - 1) * MathFunctions.PI); //Yaw
            Values.Pitch = (float)(((((float)js.PositionAxisY * 2) / jc.MaxAxisY) - 1) * MathFunctions.PI); //Pitch
            Values.Roll = (float)(((((float)js.PositionAxisX * 2) / jc.MaxAxisX) - 1) * MathFunctions.PI); //Roll

            if (Values.Speed.ToString() == float.NaN.ToString())            
                Values.Speed = 0;
            if (Values.Yaw.ToString() == float.NaN.ToString())
                Values.Yaw = 0;
            if (Values.Pitch.ToString() == float.NaN.ToString())
                Values.Pitch = 0;
            if (Values.Roll.ToString() == float.NaN.ToString())
                Values.Roll = 0;
        }
        #endregion

        #region Methods
        public override void startControlling()
        {
            ctrlTimer.Enabled = true;
        }

        public override void stopControlling()
        {
            ctrlTimer.Enabled = false;
        }
        #endregion
    }
}
